import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

hsv_low = np.array([0, 0, 0])
hsv_high = np.array([0, 0, 0])

def h_low_callback(value):
    global hsv_low
    hsv_low[0] = value

def h_high_callback(value):
    global hsv_high
    hsv_high[0] = value

def s_low_callback(value):
    global hsv_low
    hsv_low[1] = value

def s_high_callback(value):
    global hsv_high
    hsv_high[1] = value

def v_low_callback(value):
    global hsv_low
    hsv_low[2] = value

def v_high_callback(value):
    global hsv_high
    hsv_high[2] = value

def image_callback(msg):
    global flag,cv_img
    flag=True
    bridge = CvBridge()
    cv_img = bridge.imgmsg_to_cv2(msg, "bgr8")
        
cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
cv2.createTrackbar('H low', 'frame', 0, 179, h_low_callback)
cv2.createTrackbar('H high', 'frame', 0, 179, h_high_callback)
cv2.createTrackbar('S low', 'frame', 0, 255, s_low_callback)
cv2.createTrackbar('S high', 'frame', 0, 255, s_high_callback)
cv2.createTrackbar('V low', 'frame', 0, 255, v_low_callback)
cv2.createTrackbar('V high', 'frame', 0, 255, v_high_callback)

if __name__ == '__main__':
    flag=False
    cv_img=np.zeros((640,480))
    rospy.init_node("image_subscriber")

    rospy.Subscriber("image_raw", Image, image_callback)
    while not rospy.is_shutdown():
        if flag==True:
            cv2.imshow("image", cv_img)
            dst = cv2.cvtColor(cv_img, cv2.COLOR_BGR2HSV)
            dst = cv2.inRange(dst, hsv_low, hsv_high)
            cv2.imshow('dst', dst)
            cv2.waitKey(1)
    
